﻿// Copyright (c) 2024 刻BITTER
//
// This Source Code Form is subject to the terms of the Mozilla Public
// License, v. 2.0. If a copy of the MPL was not distributed with this
// file, You can obtain one at http://mozilla.org/MPL/2.0/.


#pragma once

#include <cstddef>
#include <cstdint>

#include "ioxx_data_basic.hpp"
#include "ioxx_debug.hpp"
#include "lipid_count.hpp"
#include "lipid_filter.hpp"

// DEBUG
// #include <iostream>

// DEBUG
// #include <cstdio>


#ifndef _LIPID_PID_EPSILON
    #define _LIPID_PID_EPSILON (1E-5f)  // 默认的误差死区为1e-5，误差小于死区时被置为零
#endif

// #define _LIPID_NO_PID_DEAD_ZONE  // 可以定义__LIPID_NO_PID_DEAD_ZONE 宏，关闭误差死区功能
// #define _LIPID_NO_PID_STRATEGY   // 完全关闭PID 策略，节省相应的程序资源


// lipid <= just a short word has 'pid' in it
namespace lipid {

    /**
     * @brief 表示控制器输出与被控量变化方向是否一致。
     *
     * direct: 方向一致，输出增大时被控量也增大；
     * reverse：方向相反
     *
     */
    enum class pid_direction : uint8_t {
        direct,
        reverse,
    };


    enum class d_mode : uint8_t {
        on_error,
        on_measure,
    };


    // template <size_t FilterBufferSize = 32>
    class PidCountCore {
       public:
        // using ClampType = SignalMasseuse<float, FilterBufferSize>;
        // using FilterType = typename ClampType::FilterType;

       private:
        IFilter<float, float> *_d_filter = nullptr;
        // 输入的PID 参数原始值
        float _original_kp = 0, _original_ki = 0, _original_kd = 0;
        // 用采样时间计算后的实际KI，KD 参数，KP 和时间无关
        float _timing_ki = 0, _timing_kd = 0;

        float _last_feedback = 0;
        float _last_error = 0;
        float _sample_time = 0;

        // 用来显示三项的值
        float _p_term = 0, _i_term = 0, _d_term = 0;

        // 在反馈和输出端分别附加一个环节用于限幅、映射、滤波
#ifndef _LIPID_NO_PID_DEAD_ZONE
        // 误差死区
        float _error_dead_zone = _LIPID_PID_EPSILON;
#endif

        // 积分 / 输出限幅
        float _integral_min = 0;
        float _integral_max = 0;
        float _output_min = 0;
        float _output_max = 0;

        // 运行策略
#ifndef _LIPID_NO_PID_STRATEGY
        float _i_dead_zone = 0.0f;  // 当误差大于积分死区时，积分停止
        // float _pd_dead_zone = 0.0f;  // 当误差小于PD 死区时，PD 停止
        float _dead_zone_hysteresis = 0.0f;

        float _higher_gain = 0.0f;
        float _large_error_threshold = 0.0f;
#endif

        // 运行模式
        pid_direction _direction = pid_direction::direct;
        d_mode _dm = d_mode::on_measure;  // 默认为测速反馈模式

        // 开关
        bool _i_clamp_by_output_limit = false;
        bool _i_clamp_by_integral_limit = false;
        // 输出限幅或许可以不设置开关，基本上不会有不需要限幅的场合
        // 两个枚举值和两个bool 一共4 字节，刚好四字节对齐，不需要浪费空间padding
        // bool _use_output_limit = false;

        // 状态标志
        bool _param_changed = true;
        bool _sample_time_changed = true;

#ifndef _LIPID_NO_PID_STRATEGY
        bool _use_strategy_i_dead_zone = false;
        bool _use_strategy_i_reset_when_entering_dead_zone = false;
        // bool _use_strategy_pd_dead_zone = false;

        bool _is_i_dead = false;
        // bool _is_pd_dead = false;

        bool _use_strategy_higher_gain_at_large_error = false;
        bool _is_gain_higher = false;

#endif

       public:
        /**
         * @brief Construct a new Pid Count Core object
         *
         * @param kp
         * @param ki
         * @param kd
         * @param sample_time   采样或计算周期，以秒为单位
         */
        PidCountCore(float kp, float ki, float kd, float sample_time, float out_min, float out_max) :
            _original_kp(kp),
            _original_ki(ki),
            _original_kd(kd),
            _sample_time(sample_time),
            _output_min(out_min),
            _output_max(out_max) {}

        /**
         * @brief Construct a new Pid Count Core object
         *
         * 默认 KP == KI == KD == 0，采样周期0.1s（100ms）
         *
         */
        PidCountCore() :
            PidCountCore(0.0f, 0.0f, 0.0f, 0.1f, 0.0f, 0.0f) {}

        void tune(float kp, float ki, float kd) {
            _IOXX_ASSERT_WITH_MESSAGE(!(kp < 0 || ki < 0 || kd < 0), "KP, KI, KD should be greater than zero");

            _original_kp = kp;
            _original_ki = ki;
            _original_kd = kd;

            _timing_ki = k_integral(ki);
            _timing_kd = k_differential(kd);

            _param_changed = true;
        }

        void tune(float kp, float ki, float kd, float sample_time) {
            _sample_time = sample_time;
            _sample_time_changed = true;

            this->tune(kp, ki, kd);
        }

        /**
         * @brief 对误差的积分计算可以由ki 代劳。
         *
         * @param k
         * @return float
         */
        float k_integral(float k) const {
            return k * _sample_time;
        }

        /**
         * @brief 对误差的微分计算可以由kd 代劳。
         *
         * 原理是：kd * (dE / dT) == (kd / dT) * dE，从而只需在调参时做一次除法。
         * 外部调用者可以用这个函数计算其他参数，从而与PID 对象“时间同步”
         *
         * @param k
         * @return float
         */
        float k_differential(float k) const {
            return k / _sample_time;
        }

        float count(float input, float feedback) {
            using namespace std;
            /* 参考代码: https://github.com/br3ttb/Arduino-PID-Library, License: MIT  */
            // feedback = _feedback_clamp.count(feedback);
            float error = input - feedback;
            float d_feedback = feedback - _last_feedback;
            float abs_error = abs(error);

#ifndef _LIPID_NO_PID_DEAD_ZONE
            // 忽略过小的误差
            if (abs_error < _error_dead_zone) {
                error = 0;
            }

            if (abs(d_feedback) < _error_dead_zone) {
                d_feedback = 0;
            }
#endif

            if (_direction == pid_direction::reverse) {  // 取反误差，让反向控制规律变回正向
                d_feedback = 0 - d_feedback;
                error = 0 - error;
            }

            _last_error = error;
            _last_feedback = feedback;

#ifndef _LIPID_NO_PID_STRATEGY
            float tmp_kp = _original_kp;

            if (_use_strategy_higher_gain_at_large_error) {
                if (_is_gain_higher) {
                    if (abs_error > _large_error_threshold - _dead_zone_hysteresis) {  // 积分离开死区
                        _is_gain_higher = false;
                    }

                    tmp_kp = _higher_gain;
                }
                else {
                    if (abs_error > _large_error_threshold + _dead_zone_hysteresis) {  // 积分进入死区
                        _is_i_dead = true;
                    }
                }
            }

            _p_term = tmp_kp * error;
#else
            _p_term = _original_kp * error;
#endif

            if (_dm == d_mode::on_measure) {
                _d_term = 0 - _timing_kd * d_feedback;
            }
            else {
                _d_term = _timing_kd * error;
            }

            if (_d_filter != nullptr) {
                _d_term = _d_filter->feed(_d_term);
            }

            float out = _p_term + _d_term;

            do {
#ifndef _LIPID_NO_PID_STRATEGY
                if (_use_strategy_i_dead_zone) {
                    if (_is_i_dead) {
                        if (abs_error > _i_dead_zone - _dead_zone_hysteresis) {  // 积分离开死区
                            _is_i_dead = false;
                        }

                        break;
                    }
                    else {
                        if (abs_error > _i_dead_zone + _dead_zone_hysteresis) {  // 积分进入死区
                            _is_i_dead = true;

                            if (_use_strategy_i_reset_when_entering_dead_zone) {
                                _i_term = 0;
                            }
                        }
                    }
                }
#endif

                _i_term += _timing_ki * error;

                if (_i_clamp_by_integral_limit) {
                    _i_term = value_constrain(_i_term, _integral_min, _integral_max);
                }

                out += _i_term;

                if (_i_clamp_by_output_limit) {
                    float clamped_out = value_constrain(out, _output_min, _output_max);
                    _i_term -= out - clamped_out;
                    return clamped_out;
                }
            } while (0);

            return value_constrain(out, _output_min, _output_max);
        }


        /**
         * @brief PID 和所有外部协作代码计算完成时必须调用一次，用来复位状态
         */
        void count_end() {
            _param_changed = false;
            _sample_time_changed = false;
        }

        /**
         * @brief PID 参数是否发生了修改
         *
         * 更改PID 参数后将返回 true，外部协作代码可据此决定是否要同步更新状态
         * 每次 count_end 后将自动复位为 false。
         */
        bool param_changed() const {
            return _param_changed;
        }

        /**
         * @brief 采样时间是否发生了修改
         *
         * 更改采样时间后将返回 true，外部协作代码可据此决定是否要同步更新状态，比如，根据新的采样时间重新计算与时间有关的参数。
         * 每次 count_end 后将自动复位为 false。
         */
        bool sample_time_changed() const {
            return _sample_time_changed;
        }

        void set_sample_time(float sample_time) {
            _sample_time = sample_time;
            this->tune(_original_kp, _original_ki, _original_kd);

            _sample_time_changed = true;
        }

        float get_sample_time() const {
            return _sample_time;
        }

        void set_output_limit(float min, float max) {
            _output_min = min;
            _output_max = max;
        }

        float get_kp() const {
            return _original_kp;
        }

        float get_ki() const {
            return _original_ki;
        }

        float get_kd() const {
            return _original_kd;
        }

        float get_p_term() const {
            return _p_term;
        }

        float get_i_term() const {
            return _i_term;
        }

        float get_d_term() const {
            return _d_term;
        }

        float get_error() const {
            return _last_error;
        }

        void set_d_mode(d_mode dm) {
            _dm = dm;
        }

        d_mode get_d_mode() const {
            return _dm;
        }

        void set_direction(pid_direction d) {
            _direction = d;
        }

        pid_direction get_direction() const {
            return _direction;
        }

        void enable_i_clamp_by_integral_limit(float integral_min, float integral_max) {
            _integral_min = integral_min;
            _integral_max = integral_max;
            _i_clamp_by_integral_limit = true;
        }

        void enable_i_clamp_by_integral_limit() {
            _i_clamp_by_integral_limit = true;
        }

        void disable_i_clamp_by_integral_limit() {
            _i_clamp_by_integral_limit = false;
        }

        void enable_i_clamp_by_output_limit(float out_min, float out_max) {
            _output_min = out_min;
            _output_max = out_max;
            _i_clamp_by_output_limit = true;
        }

        void enable_i_clamp_by_output_limit() {
            _i_clamp_by_output_limit = true;
        }

        void disable_i_clamp_by_output_limit() {
            _i_clamp_by_integral_limit = false;
        }

        void reset() {
            _i_term = 0;
            _last_error = 0;
            _last_feedback = 0;

            if (_d_filter != nullptr) {
                _d_filter->reset(0.0f);
            }
        }

        void stop() {
            // do nothing
        }

        /**
         *  @brief
         *  @param current_input
         *  @param current_feedback
         *  @param current_output
         */
        void resume(float current_input, float current_feedback, float current_output) {
            // 令误差归零，比例和微分项归零，然后调整积分项，使输出等于设置的输出值
            // TODO: 有点笨拙，而且没有考虑滤波器
            float current_error = current_input - current_feedback;
            _last_error = current_error;
            _last_feedback = current_feedback;

            if (_d_filter != nullptr) {
                _d_filter->reset(0.0f);
            }

            float out = count(current_input, current_feedback);
            _i_term += current_output - out;
        }

        /**
         * @brief 设置微分后的滤波器
         * @param ftr - PidCountCore 对象不会获取滤波器对象的所有权，其生命周期由调用者管理
         */
        void set_d_filter(IFilter<float, float> *ftr) {
            _d_filter = ftr;
        }

#ifndef _LIPID_NO_PID_STRATEGY

        void enable_strategy_i_dead_zone(float dead_zone) {
            using namespace std;
            _use_strategy_i_dead_zone = true;
            // dead zone 必须大于零
            //_i_dead_zone = abs(dead_zone);
            _i_dead_zone = dead_zone;
        }

        void enable_strategy_i_dead_zone() {
            _use_strategy_i_dead_zone = true;
        }

        void disable_strategy_i_dead_zone() {
            _use_strategy_i_dead_zone = false;
        }

        bool is_strategy_effective_i_dead_zone() const {
            return _use_strategy_i_dead_zone;
        }

        float get_i_dead_zone() const {
            return _i_dead_zone;
        }

        void set_error_hysteresis(float val) {
            // TODO: 迟滞不能大于死区
            _dead_zone_hysteresis = val;
        }

        float get_error_hysteresis() const {
            return _dead_zone_hysteresis;
        }

        /**
         *  @brief 使能策略：误差较大时自动替换比例系数
         *
         * 与积分死区策略共用一个迟滞参数，通过set_error_hysteresis 设置
         *
         *  @param higher_gain 替换的比例系数，非负
         *  @param threshold   误差大于阈值时替换比例系数
         */
        void enable_strategy_higher_gain_at_large_error(float higher_gain, float threshold) {
            _use_strategy_higher_gain_at_large_error = true;
            _higher_gain = higher_gain;
            _large_error_threshold = threshold;
        }

        void enable_strategy_higher_gain_at_large_error() {
            _use_strategy_higher_gain_at_large_error = true;
        }

        void disable_strategy_higher_gain_at_large_error() {
            _use_strategy_higher_gain_at_large_error = false;
        }

        bool is_strategy_effective_higher_gain_at_large_error() const {
            return _use_strategy_higher_gain_at_large_error;
        }

        void set_higher_gain(float gain) {
            _higher_gain = gain;
        }

        float get_higher_gain() const {
            return _higher_gain;
        }

        void set_large_error_threshold(float threshold) {
            _large_error_threshold = threshold;
        }

        float get_large_error_threshold() const {
            return _large_error_threshold;
        }
#endif
    };


    struct HeaterTempPoint {
        float temp;  // 温度点，每个温度点对应一组温升速度和散热功率，低于这个温度点但高于前一个温度时，
        // 采用该点对应的参数
        float temp_increment;      // 加热器温升速度，单位是℃/W・sec，即对应一瓦功率的每秒温升
        float dissipation_factor;  // 散热速度，单位是W/℃，即热阻的倒数，以室温25 ℃做参考。
        // 在室温下散热功率为0，输入功率为0 时，加热器温度保持不变
    };


    inline bool operator<(const HeaterTempPoint &left, const HeaterTempPoint &right) {
        return left.temp < right.temp;
    }


    template <size_t MaxPointCount = 10, size_t MaxDelayBufferSize = 50, size_t ParamCornerRounding = 10>
    class HeatSim {
       public:
        using DelayBufferType = MovingAverageFilter<float, float, MaxDelayBufferSize>;
        using ParamSmoothBufferType = MovingAverageFilter<float, float, ParamCornerRounding>;
        using PointPtrListType = data_basic::PointerList<HeaterTempPoint, MaxPointCount>;
        using DelayIndexType = typename DelayBufferType::WindowSizeType;

       private:
        DelayBufferType _power_delay;
        DelayBufferType _sensor_delay;
        DelayBufferType _temp_delay;  // 加热器温度如果没有滑动滤波，散热功率变化时温度会突变，导致散热功率在参数分段点附近震荡

        ParamSmoothBufferType _temp_increment_rounding;
        ParamSmoothBufferType _dissipation_factor_rounding;

        PointPtrListType _point_ptr_list;
        HeaterTempPoint _point_list[MaxPointCount] = {0};

        float _temp_heater = 0;

        float _current_heat_increment = 0;
        float _current_dissipation_factor = 0;

        float _sample_time = 0.1f;

        uint32_t _sim_counter = 0;

       public:
        HeatSim(DelayIndexType power_delay_count, DelayIndexType temp_delay_count, DelayIndexType sensor_delay_count) :
            _power_delay{0, power_delay_count},
            _sensor_delay{0, sensor_delay_count},
            _temp_delay{0, temp_delay_count} {}

        void add_point(float temp, float increment_per_watt_second, float dissipation_power_per_celsius) {
            auto size = _point_ptr_list.size();
            auto &p = _point_list[size];

            p.temp = temp;
            p.temp_increment = increment_per_watt_second;
            p.dissipation_factor = dissipation_power_per_celsius;

            _point_ptr_list.push_back(&p);
            _point_ptr_list.sort();
        }

        /**
         * @brief 获取指定温度下生效的温度参数
         *
         * @param temp
         * @return const HeaterTempPoint &若没有添加温度点，返回参数全为零
         */
        const HeaterTempPoint &get_related_point(float temp) const {
            static constexpr HeaterTempPoint zero_point = {0, 0, 0};

            if (_point_ptr_list.size() == 0) {
                return zero_point;
            }
            else {
                for (const auto &ptr_to_point : _point_ptr_list) {
                    const HeaterTempPoint &p = *ptr_to_point;
                    float t = p.temp;
                    if (t > temp) {
                        return p;
                    }
                }

                // 表里不存在温度高于temp 的点时，使用最后一个点的参数
                return _point_ptr_list.back().to_object();
            }
        }

        /**
         * @brief 用滑动滤波使参数转折处变得平滑
         *
         * @param p
         */
        void smooth_point(const HeaterTempPoint &p) {
            // TODO:
            // 如果滑动滤波的初始值为0，表示滤波器刚刚初始化，
            // 此时用添加的第一个点的数据初始化滤波器，而不是将数据喂进去

            _current_heat_increment = _temp_increment_rounding.feed(p.temp_increment);
            _current_dissipation_factor = _dissipation_factor_rounding.feed(p.dissipation_factor);
        }

        /**
         * @brief 执行一次模拟
         *
         * 模拟的初始温度为0。
         *
         * @return float 传感器输出的温度值
         */
        float sim(float input_power) {
            ++_sim_counter;

            float power = _power_delay.feed(input_power);

            const HeaterTempPoint &p = get_related_point(_temp_heater);
            smooth_point(p);

            float dissipation = _current_dissipation_factor * (_temp_heater - 25);  // 用加热器相对室温25 的温差计算散热功率

            float joule = (power - dissipation) * _sample_time;

            _temp_heater += _current_heat_increment * joule;
            _temp_heater = _temp_delay.feed(_temp_heater);

            return _sensor_delay.feed(_temp_heater);
        }

        void set_sample_time(float sec) {
            _sample_time = sec;
        }

        void set_power_delay(float init_val, size_t count) {
            _power_delay.init(count);
            _power_delay.reset(init_val);
        }

        void set_sensor_delay(float init_val, size_t count) {
            _sensor_delay.init(count);
            _sensor_delay.reset(init_val);
        }

        void set_temp_delay(float init_val, size_t count) {
            _temp_delay.init(count);
            _temp_delay.reset(init_val);
        }

        float get_heater_temp() const {
            return _temp_heater;
        }

        void set_heater_temp(float val) {
            _temp_heater = val;
            _temp_delay.reset(val);
            _sensor_delay.reset(val);
        }

        float get_sensor_temp() const {
            return _sensor_delay.get_average();
        }

        float get_dissipation_factor() const {
            return _current_dissipation_factor;
        }

        float get_temp_increment() const {
            return _current_heat_increment;
        }

        // DEBUG
        const auto &get_point_list() const {
            return _point_list;
        }

        // DEBUG
        const auto &get_point_ptr_list() const {
            return _point_ptr_list;
        }

        // DEBUG
        float get_delayed_power() const {
            return _power_delay.get_average();
        }

        /**
         * @brief 根据调用sim 的次数计算当前时间
         *
         */
        uint32_t millis() const {
            return static_cast<uint32_t>(_sim_counter * static_cast<uint32_t>(_sample_time * 1000));
        }
    };

}  // namespace lipid
